package com.yukefms.rule.formula.basis;

import com.yukefms.engine.running.RunningContext;
import com.yukefms.engine.running.MatchedStatus;
import com.yukefms.jointpoint.BodyPointCloud;
import com.yukefms.jointpoint.GeometryAdaptorForPointCalculator;
import com.yukefms.jointpoint.TolerantEpsilon;
import com.yukefms.rule.formula.interfaces.AbstractStaticBAFormula;
import com.yukefms.rule.formula.interfaces.StaticBAFormula;
import third_party.ihmc.euclid.tools.EuclidCoreTools;

/**
 * @Description
 * @Author Ping
 * @Date 2021/6/16 1:01
 **/
public class InLineFormula extends AbstractStaticBAFormula implements StaticBAFormula {

    private String jointType1 ;
    private String jointType2 ;
    private String jointType3 ;


    public InLineFormula(String jointType1, String jointType2, String jointType3) {
        super(jointType1, jointType2, jointType3) ;
        this.jointType1 = jointType1 ;
        this.jointType2 = jointType2 ;
        this.jointType3 = jointType3 ;
    }

    @Override
    public MatchedStatus matched(BodyPointCloud bodyPointCloud, RunningContext context) {
        // for test
 /*       System.out.println("Angle:" + GeometryAdaptorForPointCalculator.angle(bodyPointCloud.getPosition(jointType1),
                bodyPointCloud.getPosition(jointType2),
                bodyPointCloud.getPosition(jointType3))) ;
        System.out.println("Epsilon: " + TolerantEpsilon.ANGLE_EPSILON) ;*/
        // check if the formula is matched,
        // if it matched, then
        if (GeometryAdaptorForPointCalculator.threePointsInLine(bodyPointCloud.getPosition(jointType1),
                                                                      bodyPointCloud.getPosition(jointType2),
                                                                      bodyPointCloud.getPosition(jointType3),
                                                                      TolerantEpsilon.ANGLE_EPSILON,
                                                                      TolerantEpsilon.LENGTH_EPSILON))
        {
            // if has successor , then pass to the next formula.
            if (this.successor != null) {
                return this.successor.matched(bodyPointCloud, context);
            } else {
                return MatchedStatus.TRUE;
            }
        }
        // if does not matched
        return MatchedStatus.FALSE;
    }



}
